#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustomFastSkipStates9V)
#pragma config(Sensor, S3,     HTAC,                sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     a,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     b,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     c,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     d,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     f,             tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//#pragma autoStartTasks        // Automatically start this task when the main user program starts.
#include "drivers/CompleteDriver.h"
#define TEST_ENCOD 2000



/*void stopOffBridge (){
////////////////////////////////////////////////
forward(CHARGE_SPD);
while(nMotorEncoder(b) < ENCOD_CROSS) {}
while(true) {
stopmot();
wait1Msec(100);
if(offBridge() == true) {
stopmot();
break;
}
else if(offBridge() == false) {
DISPLAY(6, "");
forward(CHARGE_SPD);
while(nMotorEncoder(b) < E_CROSS_TRY_AGAIN) {}
}
}
}

sensorLogBegin("_Y_axis, ", "0, ", "0, ", "0, ");
  while(true) {//(!offBridge()) {
    //displayAccel();
    stopmot();
    wait1Msec(1400);

    HTACreadY(HTAC, _Y_axis);
    SensorLog(_Y_axis, 0, 0, 0);
    wait1Msec(10);
    HTACreadY(HTAC, _Y_axis);
    SensorLog(_Y_axis, 0, 0, 0);
     wait1Msec(10);
    HTACreadY(HTAC, _Y_axis);
    SensorLog(_Y_axis, 0, 0, 0);
     wait1Msec(10);
    nMotorEncoder(b) = 0;
    DISPLAY(6, "");
    forward(19);
    while(nMotorEncoder(b) < 50) {}
    if (nNxtButtonPressed == 0) {
      sensorLogEnd();
    }
  }


sensorLogBegin("WIR_VAL_BR, ", "0, ", "0, ", "0, ");
  while(p<100) {
     sensorLog(WIR_VAL_BR, 0, 0, 0);
      p++;
    showValues();sensorLogEnd();
  }
int p = 0;
*/




task main () {
  StartTask(sensorRead);
  eraseDisplay();
  bDisplayDiagnostics = false;
  eraseDisplay();
  crossBridge(false, ENCOD_CROSS, 30,true);
  stopmot();

}
